#include "MoteBridge.h"

/**
 * Creates a new instance of MoteBridge with the given settings
 * \param[in] pCollection	The collection of motes to interface with
 * \param[in] device	The device where the mote hub is connected. For example "/dev/ttyUSB0".
 * \param[in] baudrate	The baudrate for communication with the mote. For example 115200 for a telosb mote.
*/
MoteBridge::MoteBridge(MoteCollection* pCollection, const char* device,	int baudrate) 
							: collection(pCollection) {
		commInterface = new SerialComm(device, baudrate, readBuffer, writeBuffer, controlLock);
		pthread_create( &readThread, NULL, readerThreadHelper, this);
}

/**
 * Creates a new instance of MoteBridge with the given settings. 
 * Uses Telosb baudrate as default baudrate.
 * \param pCollection	The collection of motes to interface with
 * \param device	The device where the mote hub is connected. For example "/dev/ttyUSB0".
 */
MoteBridge::MoteBridge(MoteCollection* pCollection, const char* device) : collection(pCollection) {
	commInterface = new SerialComm(device, DEFAULT_BAUDRATE, readBuffer, writeBuffer, controlLock);
	pthread_create( &readThread, NULL, readerThreadHelper, this);
}

/**
 * Creates a new instance of MoteBridge with the given settings. 
 * Uses "/dev/ttyUSB0" as the default port.
 * Uses Telosb baudrate as default baudrate
 * \param pCollection	The collection of motes to interface with
*/
MoteBridge::MoteBridge(MoteCollection* pCollection) : collection(pCollection) {
	commInterface = new SerialComm(DEFAULT_DEVICE, DEFAULT_BAUDRATE, readBuffer, writeBuffer, controlLock);
	pthread_create( &readThread, NULL, readerThreadHelper, this);
}

/*!
 * Destructor of MoteBridge
*/
MoteBridge::~MoteBridge(){
	delete commInterface;
}

/*!
 * Enqueues the packet onto the write queue to be sent over the serial port.
 * Also converts the packet into a char[].
 */
bool MoteBridge::SendPacket(NetworkPacket& packet){
	if(!writeBuffer.isFull()){
		DEBUG("Sending Packet: " << packet);
		SFPacket toSend(SF_PACKET_NO_ACK);
		packet.source =(int)AM_COMPUTER_ID;
		
		toSend.setPayload(packet.str().c_str(), packet.str().size());
		return writeBuffer.enqueueBack(toSend);	
	}
	DEBUG("Unable to send packet: " << packet << "\nwriteBuffer is full");
	return false;
	
}

/*!
 * Processes a packet received from the serial port and adjust values
 * of motes as necessary.
 * \param packet The incoming packet to process
*/
void MoteBridge::processPacket(const NetworkPacket& packet){
	DEBUG("Processing Packet:" << packet);
	//Add the mote if it does not exist
	if(packet.type != BAD_PACKET){
		//Add mote tests if the packet already exists
		DEBUG("Adding Mote:" + packet.source);
		collection->addMote(packet.source);
	}
	
	switch(packet.type){
		case LIGHT_SENSOR:
			collection->changeReading(packet.data, packet.source);
			break;
		default:
			;
	}
}

/* helper function to start serial reader pthread */
void* readerThreadHelper(void* ob)
{
    static_cast<MoteBridge*>(ob)->readerThread();
    return NULL;
}

/*!
 * Processes each packet in the readBuffer queue.
*/
void MoteBridge::readerThread(){
	SFPacket queuePacket(SF_PACKET_NO_ACK);
	NetworkPacket nPacket;
	
	while(true){
		//Dequeue will block until it is not empty
		queuePacket = readBuffer.dequeue();
		nPacket = NetworkPacket(queuePacket.getPayload());
		DEBUG("Recieved Packet: " << nPacket << " from string:" << queuePacket.getLength());
		processPacket(nPacket);
	}
}

/*!
 * Sets the LEDs of the given mote (To be implemented in the future)
 * \param[in] moteId	The mote to set the LEDs
 * \param[in] values	A short that represents the LED values
 * <br>	1 for LED 0.
 * <br>	2 for LED 1.
 * <br> 4 for LED 2.
 * <br>For example, 7 is all on, and 0 is all off. 
 * \return		If the LEDs appear to be successfully set.
 */
bool MoteBridge::setLEDs(int moteId, short values){
	//to be implemented
}
	
/*!
 * Sets the sampling interval of the motes. This will do nothing in the 
 * mote simulator (To be implemented in the future)
 * \param[in] moteId the ID of the mote to set the sampling interval
 * \param[in] twentiethsOfASecond	How many twentieths of a second to wait between transmitting the light readings. 
 * \return If the sampling interval appears to have been changed.
*/
bool MoteBridge::setSamplingInterval(int moteId, int twentiethsOfASecond){
	//to be implemented
}
